/**************************************************/
void Normalize(void)
{
  float error;
  float renorm;
  static float temporary[3][3];
  //temporary[0][0] = Xorthogonal
  //temporary[1][0] = Yorthogonal
  //temporary[2][0] = Zorthogonal
  //DCM_Matrix[0][0] = X
  //DCM_Matrix[1][0] = Y
  //DCM_Matrix[2][0] = Z

  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19

  // Xorthogonal
  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19

  // Yorthogonal
  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19

  // Zorthogonal
  Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); //eq.20

  //normalize X
  renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); 
  renorm= .5 * (3-renorm);                                                 //eq.21

  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);

  //normalize Y
  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); 
  renorm= .5 * (3-renorm);                                                 //eq.21

  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);

  //normalize Z
  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); 
  renorm= .5 * (3-renorm);                                                 //eq.21

  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}

/**************************************************/
//Compensation the Roll, Pitch and Yaw drift. 
void Drift_correction(void)
{
  float Accel_magnitude; //acceleration norm
  float Accel_weight;
  float Integrator_magnitude;
  float errorCourse;
  float COGX; //Course overground X axis
  float COGY;

  //*****Roll and Pitch***************

  // Calculate the magnitude of the accelerometer vector and scale to gravity.
  Accel_magnitude = Vector_Norm(Accel_Vector) / GRAVITY;

  // Dynamic weighting of accelerometer info (reliability filter)
  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude), 0, 1);  //  

  Vector_Cross_Product(errorRollPitch, Accel_Vector, &DCM_Matrix[2][0]); //adjust the ground of reference eq 27
  Vector_Scale(Omega_P, errorRollPitch, Kp_ROLLPITCH*Accel_weight); //Proportional

  Vector_Add_Scale(Omega_I, errorRollPitch,  Ki_ROLLPITCH*Accel_weight*G_Dt); //Adding Integrator   

  //*****YAW***************
#if USE_MAGNETOMETER==1 
  // We make the gyro YAW drift correction based on compass magnetic heading
  COGX = cos(MAG_Heading);  // eq 22
  COGY = sin(MAG_Heading);  // eq 22
  errorCourse = (DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX);  //Calculating YAW error eq 23
  Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. eq 24

  Vector_Add_Scale(Omega_P, errorYaw,  Kp_YAW); //Adding Proportional
  Vector_Add_Scale(Omega_I, errorYaw,  (Ki_YAW*G_Dt)); //Adding Integrator

#else  // Use GPS Ground course to correct yaw gyro drift
  if(ground_speed >= SPEEDFILT)
  {
    COGX = cos(radians(ground_course));  // eq 22
    COGY = sin(radians(ground_course));  // eq 22
    errorCourse = (DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX);  //Calculating YAW error eq 23
    Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. eq 24

    Vector_Add_Scale(Omega_P, errorYaw,  Kp_YAW); //Adding Proportional
    Vector_Add_Scale(Omega_I, errorYaw,  (Ki_YAW*G_Dt)); //Adding Integrator
  }
#endif

  //  Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
  Integrator_magnitude = Vector_Norm(Omega_I);
  if (Integrator_magnitude > radians(300)) {
    Vector_Scale(Omega_I, Omega_I, 0.5f*radians(300)/Integrator_magnitude);
  }


}
/**************************************************/
//Compensation centrifugal acceleration
//speed_3d is speed on X plane axis
void Accel_adjust(void)
{
  Accel_Vector[1] += speed_3d*Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ
  Accel_Vector[2] -= speed_3d*Omega[1];  // Centrifugal force on Acc_z = -GPS_speed*GyroY 
}
/**************************************************/


void Matrix_update(void)
{
  float Update_Matrix[3][3]; 

  Gyro_Vector[0] = Gyro_Scaled(read_adc(0)); //gyro x roll
  Gyro_Vector[1] = Gyro_Scaled(read_adc(1)); //gyro y pitch
  Gyro_Vector[2] = Gyro_Scaled(read_adc(2)); //gyro Z yaw

/*  Accel_Vector[0] = Accel_Scale(read_adc(3)); // acc x (in m/s2)
  Accel_Vector[1] = Accel_Scale(read_adc(4)); // acc y (in m/s2)
  Accel_Vector[2] = Accel_Scale(read_adc(5)); // acc z (in m/s2)*/
  Accel_Vector[0] = read_adc(3); // acc x
  Accel_Vector[1] = read_adc(4); // acc y
  Accel_Vector[2] = read_adc(5); // acc z

  // Gyro_Vector is raw data
  // Omega is correccted with integrator term
  // Omega_vector is correccted with proportional + integrator terms

  Vector_Add(Omega, Gyro_Vector, Omega_I);  //adding Integrator term 
  Vector_Add(Omega_Vector, Omega, Omega_P); //adding Proportional term

  Accel_adjust();    //Remove centrifugal acceleration.

#if OUTPUTMODE==1         //use corrected data
  Update_Matrix[0][0]=1;
  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
  Update_Matrix[1][1]=1;
  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
  Update_Matrix[2][2]=1;
#else                    // Uncorrected data (no drift correction)
  Update_Matrix[0][0]=1;
  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
  Update_Matrix[1][1]=1;
  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
  Update_Matrix[2][2]=1;
#endif

  Matrix_Multiply(DCM_Matrix, Update_Matrix, DCM_Matrix); //Update DCM Matrix
}

void Euler_angles(void)
{
#if (OUTPUTMODE==2)         // Only accelerometer info (debugging purposes)
  roll = atan2(Accel_Vector[1], Accel_Vector[2]);    // atan2(acc_y,acc_z)
  pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
  yaw = 0;
#else
  roll = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
  pitch = -asin(DCM_Matrix[2][0]);
  yaw = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
#endif
}

void DCM(void)
{
  Matrix_update(); 
  Normalize();
  Drift_correction();
  Euler_angles();
}





